package edu.arizona.sista.learn.util;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import edu.arizona.sista.learn.model.Interval;
import edu.arizona.sista.learn.model.holding.Person;
import edu.arizona.sista.learn.model.holding.Object;
import edu.arizona.sista.learn.model.motion.PathWeight;
import edu.arizona.sista.learn.model.motion.Track;


public class Computable {
	
	/**
	 * Compute the probability of two spatial things being connected given their positions at times t and
	 * t + 1. This probability is actually the intersection of two probability estimates, that depend
	 * respectively on the proximity of the spatial things and on the movement of the spatial things.
	 * @param coordinates11 coordinates of the first spatial thing at time t
	 * @param coordinates21 coordinates of the second spatial thing at time t
	 * @param coordinates12 coordinates of the first spatial thing at time t + 1
	 * @param coordinates22 coordinates of the second spatial thing at time t + 1
	 * @param dim11 dimensions of the first spatial thing at time t
	 * @param dim21 dimensions of the second spatial thing at time t
	 * @param dim12 dimensions of the first spatial thing at time t + 1
	 * @param dim22 dimensions of the second spatial thing at time t + 1
	 * @return a double between 0 and 1 representing the probability of a connection.
	 */	
	public static double connected(
			float[] coordinate11, float[] coordinate21, float[] coordinate12, float[] coordinate22,
			int[] dim11, int[] dim21, int[] dim12, int[] dim22){
		
		double prob1 = proximityProbability(
				coordinate11, coordinate21, coordinate12, coordinate22,
				dim11, dim21, dim12, dim22);
		
		double prob2 = movementProbability(
				coordinate11, coordinate21, coordinate12, coordinate22,
				dim11, dim12);

		return prob1 * prob2;
	}

	/**
	 * Compute the probability of two spatial things being connected according to their movement.
	 * The final probability computation includes a normalization with the area of the first object,
	 * because the first object is supposed to be a hand, and a hand has a relatively constant size.
	 * Notice that spatial things are often motionless, and thus connected to each other in terms of
	 * movement. So this probability estimation is only useful when the spatial things are moving.
	 * @param coordinates11 coordinates of the first spatial thing at time t
	 * @param coordinates21 coordinates of the second spatial thing at time t
	 * @param coordinates12 coordinates of the first spatial thing at time t + 1
	 * @param coordinates22 coordinates of the second spatial thing at time t + 1
	 * @param dim11 dimensions of the first spatial thing at time t
	 * @param dim12 dimensions of the first spatial thing at time t + 1
	 * @return a double between 0 and 1 representing the probability of a connection.
	 */
	private static double movementProbability(
			float[] coordinate11, float[] coordinate21, float[] coordinate12, float[] coordinate22,
			int[] dim11, int[] dim12) {

		//Modeled using the euclidean distance
		
		//U is the first movement vector (the hand)
		double xU = coordinate12[0] - coordinate11[0];
		double yU = coordinate12[1] - coordinate11[1];
		
		//V is the second movement vector
		double xV = coordinate22[0] - coordinate21[0];
		double yV = coordinate22[1] - coordinate21[1];
		
		//Euclidean distance
		double d = Math.sqrt(Math.pow(xU - xV, 2) + Math.pow(yU - yV, 2));
		
	    //The probability is dependent on the angle, the norm and the radius of the circle of the
		//first spatial thing (which is supposed to correspond to the hand)
		double D1 = dim11[0] + dim12[0] / 4;
		double H1 = dim11[1] + dim12[1] / 4;
		double radius = Math.max(D1, H1);
	    
		return Math.exp(- Math.pow(2 * d / radius, 2));
	}

	/**
	 * Compute the probability of two spatial things being connected according to their proximity.
	 * The final probability computation includes a normalization with the area of the first object,
	 * because the first object is supposed to be a hand, and a hand has a relatively constant size.
	 * @param coordinates11 coordinates of the first spatial thing at time t
	 * @param coordinates21 coordinates of the second spatial thing at time t
	 * @param coordinates12 coordinates of the first spatial thing at time t + 1
	 * @param coordinates22 coordinates of the second spatial thing at time t + 1
	 * @param dim11 dimensions of the first spatial thing at time t
	 * @param dim21 dimensions of the second spatial thing at time t
	 * @param dim12 dimensions of the first spatial thing at time t + 1
	 * @param dim22 dimensions of the second spatial thing at time t + 1
	 * @return a double between 0 and 1 representing the probability of a connection.
	 */
	private static double proximityProbability(
			float[] coordinate11, float[] coordinate21, float[] coordinate12, float[] coordinate22,
			int[] dim11, int[] dim21, int[] dim12, int[] dim22) {

		//The maximum distance must be the sum of the mean "radius" of the ellipses of the objects
		//The ellipses correspond to the half of the inellipses of the bounding boxes associated to
		//the objects (a simple inellipse is not restrictive enough)  
		double D1 = dim11[0] + dim12[0] / 4;
		double H1 = dim11[1] + dim12[1] / 4;
		double D2 = dim21[0] + dim22[0] / 4;
		double H2 = dim21[1] + dim22[1] / 4;
		double D = (D1 + D2) / 2;
		double H = (H1 + H2) / 2;
		
		//A is the mean center of the first object (supposedly the hand)
		double xA = coordinate11[0] + coordinate12[0] / 2;
		double yA = coordinate11[1] + coordinate12[1] / 2;
		
		//B is the mean center of the second object
		double xB = coordinate21[0] + coordinate22[0] / 2;
		double yB = coordinate21[1] + coordinate22[1] / 2;
		
	    //Vector AB
	    double xAB = xB - xA;
	    double yAB = yB - yA;
	    double lAB2 = xAB * xAB + yAB * yAB;
	    double lAB = Math.sqrt(lAB2);
	    if (lAB == 0) return 1;

	    //Sum of the "radius" of the ellipses
	    double sumR;
	    if (yAB == 0) sumR = D;
	    else if (xAB == 0) sumR = H;
	    else{
	    	double sumRx = Math.sqrt(1 / ((1 / Math.pow(D, 2)) + (Math.pow(xAB / (H * yAB), 2))));
	    	double sumRy = sumRx * xAB / yAB;
	    	double sumR2 = sumRx * sumRx + sumRy * sumRy;
	    	sumR = Math.sqrt(sumR2);
	    }
	    if (lAB < sumR) return 1;

		//The distance between the ellipses is then lAB - sumR
	    double dist = lAB - sumR;

	    //The probability is dependent on the distance between the ellipses and the area of the
	    //first ellipse (the first spatial thing is supposed to correspond to the hand)
	    double area = Math.PI * D1 * H1;
	    double prob = Math.exp(- Math.pow(750 * dist / area, 2));
		return prob;
	}

	/**
	 * Test whether an object is between two people during some time interval. The object is considered as
	 * being between the two people in a unidimensional way: it just has to be at the right of the person on
	 * the left and at the left of the person on the right on the screen.
	 * @param p1 first person
	 * @param p2 second person
	 * @param o the object
	 * @param t time interval
	 * @return true if the object is between p1 and p2 during the time interval
	 */
	public static boolean between(Person p1, Person p2, Object o, Interval t) {
		// Here I suppose that the three spatial things p1, p2 and o have the same temporal extent
		for(int i = t.start(); i < t.end(); i++){
			float xP1 = p1.getCoordinate(i)[0];
			float xP2 = p2.getCoordinate(i)[0];
			float xO = o.getCoordinate(i)[0];
			float left = Math.min(xP1, xP2);
			float right = Math.max(xP1, xP2);
			if (xO < left || xO > right) return false;
		}
		return true;
	}
	
	/**
	 * Compute linear regressions. Inspired from (Sedgewick and Wayne, 2008). 
	 * @param points
	 * @return the input points projected on the line and the goodness of fit. 
	 */
	
	public static PathWeight linearRegression(List<Track> path){
        // first pass: read in data, compute xbar and ybar
        double sumx = 0.0, sumy = 0.0;
        for(Track track: path){
        	double[] p = track.pos;
            sumx  += p[0];
            sumy  += p[1];
        }
        double xbar = sumx / path.size();
        double ybar = sumy / path.size();
        
        // second pass: compute summary statistics
        double xxbar = 0.0, yybar = 0.0, xybar = 0.0;
        for (Track track: path) {
        	double[] p = track.pos;
            xxbar += (p[0] - xbar) * (p[0] - xbar);
            yybar += (p[1] - ybar) * (p[1] - ybar);
            xybar += (p[0] - xbar) * (p[1] - ybar);
        }
        
        //y = beta1 * x + beta0
        double beta1 = xybar / xxbar;
        double beta0 = ybar - beta1 * xbar;
        
        // goodness of fit
        double ssr = 0.0; // regression sum of squares
        for (Track track: path) {
        	double[] p = track.pos;
            double fit = beta1 * p[0] + beta0;
            ssr += (fit - ybar) * (fit - ybar);
        }
        double R2 = ssr / yybar;

        //projections
        List<Track> list = new ArrayList<Track>();
        for(Track track: path) list.add(new Track(track.frame, project(track.pos, beta1, beta0)));
        
        PathWeight pw = new PathWeight(list, R2);
        return pw;
	}

	/**
	 * Project a point on a line.
	 * @param p point to project
	 * @param a
	 * @param b
	 * @return the projected point
	 */
	private static double[] project(double[] p, double a, double b) {
		double[] g = new double[2];
        g[0] = (p[1] + (1 / a) * p[0] - b) / (a + (1 / a));
		g[1] = a * g[0] + b;
		return g;
	}
	
}
